/**
  ******************************************************************************
  * @file           : shoot_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/1/18
  ******************************************************************************
  */



#include "shoot_task.h"

extern rc_device_t g_rc;
extern judge_info_0 judgeInfo0;
extern chassis_info chassisInfo;
extern sentry_state normal_state;

float friction_motor_speed_set = 0.75;
float feed_motor_rounds_set = 0;
uint8_t is_vision_shoot_permit = 0; //打出子弹后自动置0
int32_t permitted_firing_capacity = 0; //每100ms允许发弹量
uint16_t fired_ammunition_count = 0;    //每100ms已发弹量

/**--------------------------摩擦轮-----------------------------**/

void friction_motor_task(void *argument){
    static DJI_motor friction_motor_left(&hcan1, nullptr, 1, DJI_M3508, 0);
    static DJI_motor friction_motor_right(&hcan1, nullptr, 2, DJI_M3508, 1);
    friction_motor_left.velPid.pid_reset(1.0, 0.05, 12, 0 ,0);
    friction_motor_right.velPid.pid_reset(1.0, 0.05, 12, 0 ,0);
    for(;;){
        if(g_rc.is_lost == 0 && chassisInfo.is_flip_over == 0){
            if(judgeInfo0.mains_power_shooter_output == 1) {
                if (((g_rc.rc_info.sw.sw_l == RC_SW_MID || g_rc.rc_info.sw.sw_l == RC_SW_DOWN) && g_rc.rc_info.sw.sw_r == RC_SW_DOWN)
                || (g_rc.rc_info.sw.sw_r == RC_SW_UP && g_rc.rc_info.sw.sw_l == RC_SW_MID)) {
                    ///右推杆拨下、左推杆拨中或下时左头摩擦轮才转
                    friction_motor_left.motor_set_speed(friction_motor_speed_set);
                    friction_motor_right.motor_set_speed(friction_motor_speed_set);
//                    friction_motor_left.motor_set_current(0);
//                    friction_motor_right.motor_set_current(0);
                } else {
                    friction_motor_left.motor_set_speed(0);
                    friction_motor_right.motor_set_speed(0);
                }
            }else{ //手动断电
                friction_motor_left.motor_set_current(0);
                friction_motor_right.motor_set_current(0);
            }
        }else{
            friction_motor_left.motor_set_current(0);
            friction_motor_right.motor_set_current(0);
        }
        osDelay(1);
    }
}

/**--------------------------拨弹电机-----------------------------**/

uint32_t tick_cnt = 0;
uint32_t stall_tick = 0;
bullet_feed_motor_status status = init;
float bullet_real_current = 0;
float bullet_last_current = 0;
uint8_t is_stall = 0;

void stall_protection(DJI_motor &motor){ //堵转保护
    bullet_last_current = bullet_real_current;
    bullet_real_current = motor.motor_get_current();
    if(bullet_real_current > 9000 && bullet_last_current <= 9000){
        feed_motor_rounds_set = motor.motor_get_rounds() - ONE_BULLET_ROUNDS / 5.f;
        is_stall = 1;
    }
}


uint8_t shoot_permit_heat_test = 0;
uint16_t cnt_test = 0;
uint32_t fire_interval_time = 0;
void bullet_feed_task(void *argument){
    static DJI_motor bullet_feed_motor(&hcan1, nullptr, 3, DJI_M2006, 0);
    bullet_feed_motor.velPid.pid_reset(1.0f, 0.1f, 12.f, 0.00f, 1.f);
    bullet_feed_motor.posPid.pid_reset(0.25f, 0.1f, 0.2f, 0, 0.01f);
    tick_cnt = 0;
    for(;;){
        if(g_rc.is_lost == 0 && chassisInfo.is_flip_over == 0){
            if(judgeInfo0.mains_power_shooter_output == 1) {
                if (((g_rc.rc_info.sw.sw_l == RC_SW_DOWN || g_rc.rc_info.sw.sw_l == RC_SW_MID) && g_rc.rc_info.sw.sw_r == RC_SW_DOWN)
                || (g_rc.rc_info.sw.sw_r == RC_SW_UP && g_rc.rc_info.sw.sw_l == RC_SW_MID)) {   //需摩擦轮开启，单发模式
                    if (((is_sw_event(&g_rc, RC_SW_L_MID2DOWN) && g_rc.rc_info.sw.sw_r == RC_SW_DOWN) || is_vision_shoot_permit == 1 || shoot_permit_heat_test == 1)
                        && (permitted_firing_capacity > 0) && (fire_interval_time > MIN_FIRE_INTERVAL)) {
                        feed_motor_rounds_set = feed_motor_rounds_set + ONE_BULLET_ROUNDS + is_stall * ONE_BULLET_ROUNDS / 5.f;
                        is_stall = 0;
                        fired_ammunition_count += 1; //发弹量+1
                        is_vision_shoot_permit = 0;
                        shoot_permit_heat_test = 0;
                        fire_interval_time = 0;
                        permitted_firing_capacity--;
                    }
//                    stall_protection(bullet_feed_motor);
                    bullet_feed_motor.motor_set_rounds(feed_motor_rounds_set);
                } else {
                    feed_motor_rounds_set = bullet_feed_motor.motor_get_rounds();
                    bullet_feed_motor.motor_set_speed(0);
                }
            }else{
                bullet_feed_motor.motor_set_current(0);
            }
        }else{
            bullet_feed_motor.motor_set_current(0);
        }
        /**     热量测试     **/
#if     HEAT_DEBUG
        cnt_test ++;
        if(cnt_test > 100){
            shoot_permit_heat_test = 1;
            cnt_test = 0;
        }

#endif

        fire_interval_time++;


        osDelay(1);
    }
}
